
#include <BCone>
#include <BCube>
#include <BCapsule>
#include <BCylinder>
#include <BSphere>
#include <BGeoSphere>
#include <BTube>
#include <BTorus>
#include <BSurface>
#include <BTerrain>
#include <BPhysics>
#include <BPointConstraint>
#include <BHingeConstraint>
#include <BSliderConstraint>
#include <BConearConstraint>
#include <BSixdofConstraint>

#include <btStaticPlaneShape.h>
#include "DynamicScene.h"

DynamicScene::DynamicScene()
{
	_dispatcher = new btCollisionDispatcher();
	_broadphase = new btDbvtBroadphase();
	_solver = new btSequentialConstraintSolver;
	_dynamicsWorld = new btDiscreteDynamicsWorld(_dispatcher, _broadphase, _solver);
	_dynamicsWorld->setGravity(btVector3(0, 0, -10));

	btStaticPlaneShape* staticPlane = new btStaticPlaneShape(btVector3(0, 0, 1), 0);
	btRigidBody* body = new btRigidBody();
	body->setShape(staticPlane);
	_dynamicsWorld->addBody(body);

	this->setAsset(_dynamicsWorld);

	connect(this, Signal_Insert, &DynamicScene::slotInsert);
	connect(this, Signal_Remove, &DynamicScene::slotRemove);
}
DynamicScene::~DynamicScene()
{

}

btConstraint* DynamicScene::createTypedConstraint(BConstraint* constraint)
{
	const char* className = constraint->className();
	if (constraint->typed("BPointConstraint"))
	{
		BPointConstraint* pointConstraint = (BPointConstraint*)constraint;
		btRigidBody* rigidBodyA = pointConstraint->nodeA()->asset();
		btRigidBody* rigidBodyB = pointConstraint->nodeB()->asset();

		const BVector& pointA = pointConstraint->pointA();
		const BVector& pointB = pointConstraint->pointB();
		btVector3 pivotInA(pointA.x(), pointA.y(), pointA.z());
		btVector3 pivotInB(pointB.x(), pointB.y(), pointB.z());

		btPointConstraint* constraint = new btPointConstraint(rigidBodyA, rigidBodyB, pivotInA, pivotInB);
		//constraint->m_setting.m_tau = point2Constraint->tau();
		//constraint->m_setting.m_damping = point2Constraint->damping();
		//constraint->m_setting.m_impulseClamp = point2Constraint->impulseClamp();
		return constraint;
	}
	if (constraint->typed("BHingeConstraint"))
	{
		BHingeConstraint* hingeConstraint = (BHingeConstraint*)constraint;
		btRigidBody* rigidBodyA = hingeConstraint->nodeA()->asset();
		btRigidBody* rigidBodyB = hingeConstraint->nodeB()->asset();

		const BVector& pointA = hingeConstraint->pointA();
		const BVector& pointB = hingeConstraint->pointB();
		btVector3 pivotInA(pointA.x(), pointA.y(), pointA.z());
		btVector3 pivotInB(pointB.x(), pointB.y(), pointB.z());

		const BVector& axisA = hingeConstraint->axisA();
		const BVector& axisB = hingeConstraint->axisB();
		btVector3 axisInA(axisA.x(), axisA.y(), axisA.z());
		btVector3 axisInB(axisB.x(), axisB.y(), axisB.z());

		btHingeConstraint* constraint = new btHingeConstraint(rigidBodyA, rigidBodyB, pivotInA, pivotInB, axisInA, axisInB);
		{
			//constraint->enableMotor(hingeConstraint->motorEnabled());
			//constraint->setMaxMotorImpulse(hingeConstraint->motorMaxImpulse());
			//constraint->setMotorTargetVelocity(hingeConstraint->motorTargetVelocity());
			//const BAngleLimit& limit = hingeConstraint->angleLimit();
			//BReal low = limit.low();
			//BReal high = limit.high();
			//BReal softness = limit.softness();
			//BReal biasFactor = limit.biasFactor();
			//BReal relaxFactor = limit.relaxFactor();
			//constraint->setLimit(low, high, softness, biasFactor, relaxFactor);
		}
		return constraint;
	}
	if (constraint->typed("BSliderConstraint"))
	{
		BSliderConstraint* sliderConstraint = (BSliderConstraint*)constraint;
		btRigidBody* bodyA = sliderConstraint->nodeA()->asset();
		btRigidBody* bodyB = sliderConstraint->nodeB()->asset();
		bodyA->setActivationState(Disable_Deactivation);
		bodyB->setActivationState(Disable_Deactivation);

		BVector pointA = sliderConstraint->pointA();
		BQuater rotateA(BVector(1, 0, 0), sliderConstraint->axisA());
		btTransform frameInA = btTransform::getIdentity();
		frameInA.setOrigin(btVector3(pointA.x(), pointA.y(), pointA.z()));
		frameInA.setRotation(btQuaternion(rotateA.x(), rotateA.y(), rotateA.z(), rotateA.w()));

		const BVector& pointB = sliderConstraint->pointB();
		BQuater rotateB(BVector(1, 0, 0), sliderConstraint->axisB());
		btTransform frameInB = btTransform::getIdentity();
		frameInB.setOrigin(btVector3(pointB.x(), pointB.y(), pointB.z()));
		frameInB.setRotation(btQuaternion(rotateB.x(), rotateB.y(), rotateB.z(), rotateB.w()));

		btSliderConstraint* constraint = new btSliderConstraint(bodyA, bodyB, frameInA, frameInB);
		{
			const BRealRange& linearLimit = sliderConstraint->linearLimit();
			constraint->setLowerLinearLimit(linearLimit.min());
			constraint->setUpperLinearLimit(linearLimit.max());

			const BRealRange& angleLimit = sliderConstraint->angleLimit();
			constraint->setLowerAngularLimit(angleLimit.min());
			constraint->setUpperAngularLimit(angleLimit.max());
		}
		return constraint;
	}
	if (constraint->typed("BConearConstraint"))
	{
		BConearConstraint* conearConstraint = (BConearConstraint*)constraint;
		btRigidBody* bodyA = conearConstraint->nodeA()->asset();
		btRigidBody* bodyB = conearConstraint->nodeB()->asset();
		bodyA->setActivationState(Disable_Deactivation);
		bodyB->setActivationState(Disable_Deactivation);

		const BVector& pointA = conearConstraint->pointA();
		const BVector& pointB = conearConstraint->pointB();

		btTransform frameInA = btTransform::getIdentity();
		frameInA.setOrigin(btVector3(pointA.x(), pointA.y(), pointA.z()));
		frameInA.getBasis().setEulerZYX(0, 0, PI / 2);
		btTransform frameInB = btTransform::getIdentity();
		frameInB.setOrigin(btVector3(pointB.x(), pointB.y(), pointB.z()));
		//frameInB.setRotation(btQuaternion(rotateB.x(), rotateB.y(), rotateB.z(), rotateB.w()));

		btConeTwistConstraint* constraint = new btConeTwistConstraint(bodyA, bodyB, frameInA, frameInB);
		BReal twistSpan = conearConstraint->twistSpan();
		BReal swingSpan1 = conearConstraint->swingSpan1();
		BReal swingSpan2 = conearConstraint->swingSpan2();
		BReal softness = conearConstraint->softness();
		BReal biasFactor = conearConstraint->biasFactor();
		BReal relaxFactor = conearConstraint->relaxFactor();
		//constraint->setLimit(swingSpan1, swingSpan2, twistSpan, softness, biasFactor, relaxFactor);
		constraint->setLimit(swingSpan1, swingSpan2, twistSpan, 0.5f);
		return constraint;
	}
	if (constraint->typed("BSixdofConstraint"))
	{
		BSixdofConstraint* sixdofConstraint = (BSixdofConstraint*)constraint;
		btRigidBody* bodyA = sixdofConstraint->nodeA()->asset();
		btRigidBody* bodyB = sixdofConstraint->nodeB()->asset();
		bodyA->setActivationState(Disable_Deactivation);
		bodyB->setActivationState(Disable_Deactivation);

		const BVector& pointA = sixdofConstraint->pointA();
		btTransform frameInA = btTransform::getIdentity();
		frameInA.setOrigin(btVector3(pointA.x(), pointA.y(), pointA.z()));

		const BVector& pointB = sixdofConstraint->pointB();
		btTransform frameInB = btTransform::getIdentity();
		frameInB.setOrigin(btVector3(pointB.x(), pointB.y(), pointB.z()));

		btSixdofConstraint* constraint = new btSixdofConstraint(bodyA, bodyB, frameInA, frameInB, true);
		{
			const BVector& linearLowerLimit = sixdofConstraint->linearLowerLimit();
			const BVector& linearUpperLimit = sixdofConstraint->linearUpperLimit();
			const BVector& angularLowerLimit = sixdofConstraint->angularLowerLimit();
			const BVector& angularUpperLimit = sixdofConstraint->angularUpperLimit();
			constraint->setLinearLowerLimit(btVector3(linearLowerLimit.x(), linearLowerLimit.y(), linearLowerLimit.z()));
			constraint->setLinearUpperLimit(btVector3(linearUpperLimit.x(), linearUpperLimit.y(), linearUpperLimit.z()));
			constraint->setAngularLowerLimit(btVector3(angularLowerLimit.x(), angularLowerLimit.y(), angularLowerLimit.z()));
			constraint->setAngularUpperLimit(btVector3(angularUpperLimit.x(), angularUpperLimit.y(), angularUpperLimit.z()));
		}
		return constraint;
	}
	return 0;
}
btShape* DynamicScene::createCollisionShape(BShape* shape)
{
	if (BCone* cone = dynamic_cast<BCone*>(shape))
	{
		btShape* colShape = new btConeShapeZ(cone->radius(), cone->height() / 2);
		return colShape;
	}
	if (BCube* cube = dynamic_cast<BCube*>(shape))
	{
		btVector3 halfExtents(cube->length() / 2, cube->width() / 2, cube->height() / 2);
		btShape* colShape = new btBoxShape(halfExtents);
		return colShape;
	}
	if (BCapsule* capsule = dynamic_cast<BCapsule*>(shape))
	{
		btShape* colShape = new btCapsuleShapeZ(capsule->radius(), capsule->height());
		return colShape;
	}
	if (BCylinder* cylinder = dynamic_cast<BCylinder*>(shape))
	{
		btVector3 halfExtents(cylinder->radius(), cylinder->radius(), cylinder->height());
		btShape* colShape = new btCylinderShapeZ(halfExtents);
		return colShape;
	}
	if (BSphere* sphere = dynamic_cast<BSphere*>(shape))
	{
		btShape* colShape = new btSphereShape(sphere->radius());
		return colShape;
	}
	if (BTube* tube = dynamic_cast<BTube*>(shape))
	{
		btVector3 halfExtents(tube->radius() / 2, tube->radius() / 2, tube->height() / 2);
		btShape* colShape = new btCylinderShape(halfExtents);
		return colShape;
	}
	if (BGeoSphere* sphere = dynamic_cast<BGeoSphere*>(shape))
	{
		btShape* colShape = new btSphereShape(sphere->radius());
		return colShape;
	}
	if (BMesh* mesh = dynamic_cast<BMesh*>(shape))
	{
		btTriangleMesh* meshInterface = new btTriangleMesh();
		BVectorArray* vertices = mesh->vertices();
		BFaceArray* faces = mesh->faces();
		for (int i = 0; i < faces->size(); i++)
		{
			const BFace& face = faces->at(i);
			const BVector& v0 = vertices->at(face.a());
			const BVector& v1 = vertices->at(face.b());
			const BVector& v2 = vertices->at(face.c());
			meshInterface->addTriangle(btVector3(v0.x(), v0.y(), v0.z()), btVector3(v1.x(), v1.y(), v1.z()), btVector3(v2.x(), v2.y(), v2.z()));
		}
		btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
		return trimesh;
	}
	if (BGeometry* geometry = dynamic_cast<BGeometry*>(shape))
	{
		btTriangleMesh* triangleMesh = new btTriangleMesh();
		BVectorArray* vertices = geometry->vertices();
		for (int e = 0; e < geometry->elementCount(); e++)
		{
			BElement* element = geometry->element(e);
			if (element->primitive() == Primitive_Triangles)
			{
				if (BIndexArray* indices = element->indices())
				{
					int numTrigs = indices->size() / 3;
					for (int i = 0; i < numTrigs; i++)
					{
						int ipos = i * 3;
						const BVector& v0 = vertices->at(indices->at(ipos + 0));
						const BVector& v1 = vertices->at(indices->at(ipos + 1));
						const BVector& v2 = vertices->at(indices->at(ipos + 2));
						triangleMesh->addTriangle(btVector3(v0.x(), v0.y(), v0.z()), btVector3(v1.x(), v1.y(), v1.z()), btVector3(v2.x(), v2.y(), v2.z()));
					}
				}
				if (BPieceArray* pieces = element->pieces())
				{
					for (int i = 0; i < pieces->size(); i += 3)
					{
						const BPiece& pa = (*pieces)[i + 0];
						const BPiece& pb = (*pieces)[i + 1];
						const BPiece& pc = (*pieces)[i + 2];
						const BVector& va = (*vertices)[pa.v()];
						const BVector& vb = (*vertices)[pb.v()];
						const BVector& vc = (*vertices)[pc.v()];
						triangleMesh->addTriangle(btVector3(va.x(), va.y(), va.z()), btVector3(vb.x(), vb.y(), vb.z()), btVector3(vc.x(), vc.y(), vc.z()));
					}
					continue;
				}
				int stop = element->first() + element->count();
				for (int i = element->first(); i < stop; i += 3)
				{
					const BVector& va = (*vertices)[i + 0];
					const BVector& vb = (*vertices)[i + 1];
					const BVector& vc = (*vertices)[i + 2];
					triangleMesh->addTriangle(btVector3(va.x(), va.y(), va.z()), btVector3(vb.x(), vb.y(), vb.z()), btVector3(vc.x(), vc.y(), vc.z()));
				}
			}
			if (element->primitive() == Primitive_Quads)
			{
				if (BIndexArray* indices = element->indices())
				{
					int numQuads = indices->size() / 4;
					for (int i = 0; i < numQuads; i++)
					{
						int ipos = i * 4;
						const BVector& va = vertices->at(indices->at(ipos + 0));
						const BVector& vb = vertices->at(indices->at(ipos + 1));
						const BVector& vc = vertices->at(indices->at(ipos + 2));
						const BVector& vd = vertices->at(indices->at(ipos + 3));
						triangleMesh->addTriangle(btVector3(va.x(), va.y(), va.z()), btVector3(vb.x(), vb.y(), vb.z()), btVector3(vc.x(), vc.y(), vc.z()));
						triangleMesh->addTriangle(btVector3(va.x(), va.y(), va.z()), btVector3(vc.x(), vc.y(), vc.z()), btVector3(vd.x(), vd.y(), vd.z()));
					}
					continue;
				}
				if (BPieceArray* pieces = element->pieces())
				{
					for (int i = 0; i < pieces->size(); i += 4)
					{
						const BPiece& pa = (*pieces)[i + 0];
						const BPiece& pb = (*pieces)[i + 1];
						const BPiece& pc = (*pieces)[i + 2];
						const BPiece& pd = (*pieces)[i + 3];
						const BVector& va = (*vertices)[pa.v()];
						const BVector& vb = (*vertices)[pb.v()];
						const BVector& vc = (*vertices)[pc.v()];
						const BVector& vd = (*vertices)[pd.v()];
						triangleMesh->addTriangle(btVector3(va.x(), va.y(), va.z()), btVector3(vb.x(), vb.y(), vb.z()), btVector3(vc.x(), vc.y(), vc.z()));
						triangleMesh->addTriangle(btVector3(va.x(), va.y(), va.z()), btVector3(vc.x(), vc.y(), vc.z()), btVector3(vd.x(), vd.y(), vd.z()));
					}
					continue;
				}
				int stop = element->first() + element->count();
				for (int i = element->first(); i < stop; i += 3)
				{
					const BVector& va = (*vertices)[i + 0];
					const BVector& vb = (*vertices)[i + 1];
					const BVector& vc = (*vertices)[i + 2];
					const BVector& vd = (*vertices)[i + 3];
					triangleMesh->addTriangle(btVector3(va.x(), va.y(), va.z()), btVector3(vb.x(), vb.y(), vb.z()), btVector3(vc.x(), vc.y(), vc.z()));
					triangleMesh->addTriangle(btVector3(va.x(), va.y(), va.z()), btVector3(vc.x(), vc.y(), vc.z()), btVector3(vd.x(), vd.y(), vd.z()));
				}
			}
		}
		btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(triangleMesh, true, true);
		return trimesh;
	}
	if (BSurface* surface = dynamic_cast<BSurface*>(shape))
	{
		btTriangleMesh* meshInterface = new btTriangleMesh();
		BVectorArray* vertices = surface->vertices();
		BIndexArray* indices = surface->indices();
		int numQuads = indices->size() / 4;
		for (int i = 0; i < numQuads; i++)
		{
			const BVector& v0 = vertices->at(indices->at(i * 4 + 0));
			const BVector& v1 = vertices->at(indices->at(i * 4 + 1));
			const BVector& v2 = vertices->at(indices->at(i * 4 + 2));
			const BVector& v3 = vertices->at(indices->at(i * 4 + 3));
			meshInterface->addTriangle(btVector3(v0.x(), v0.y(), v0.z()), btVector3(v1.x(), v1.y(), v1.z()), btVector3(v2.x(), v2.y(), v2.z()));
			meshInterface->addTriangle(btVector3(v0.x(), v0.y(), v0.z()), btVector3(v2.x(), v2.y(), v2.z()), btVector3(v3.x(), v3.y(), v3.z()));
		}
		btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
		return trimesh;
	}
	if (BTerrain* terrain = dynamic_cast<BTerrain*>(shape))
	{
		btTriangleMesh* meshInterface = new btTriangleMesh();
		BVectorArray* vertices = terrain->vertices();
		BIndexArray* indices = terrain->indices();
		int numQuads = indices->size() / 4;
		for (int i = 0; i < numQuads; i++)
		{
			const BVector& v0 = vertices->at(indices->at(i * 4 + 0));
			const BVector& v1 = vertices->at(indices->at(i * 4 + 1));
			const BVector& v2 = vertices->at(indices->at(i * 4 + 2));
			const BVector& v3 = vertices->at(indices->at(i * 4 + 3));
			meshInterface->addTriangle(btVector3(v0.x(), v0.y(), v0.z()), btVector3(v1.x(), v1.y(), v1.z()), btVector3(v2.x(), v2.y(), v2.z()));
			meshInterface->addTriangle(btVector3(v0.x(), v0.y(), v0.z()), btVector3(v2.x(), v2.y(), v2.z()), btVector3(v3.x(), v3.y(), v3.z()));
		}
		btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
		return trimesh;
	}
	return 0;
}
btRigidBody* DynamicScene::createRigidBody(BNode* node, BPhysics* physics)
{
	const BVector& origin = node->position();
	const BQuater& rotate = node->rotate();
	btTransform transform;
	transform.setOrigin(btVector3(origin.x(), origin.y(), origin.z()));
	transform.setRotation(btQuaternion(rotate.x(), rotate.y(), rotate.z(), rotate.w()));
	double mass = physics->mass();
	const BVector& force = physics->force();
	if (node->shapeCount() == 1)
	{
		btVector3 localInertia(0, 0, 0);
		BShape* shape = node->shape(0);
		btShape* colShape = createCollisionShape(shape);
		if (mass != 0)
			colShape->calculateLocalInertia(mass, localInertia);
		btRigidBody* body = new btRigidBody();
		body->setMass(mass);
		body->setInertia(localInertia);
		body->setShape(colShape);
		body->setTransform(transform);
		body->setForce(btVector3(force.x(), force.y(), force.z()));
		body->setName(node->name().cstr());
		body->setAsset(node);
		node->setAsset(body);
		return body;
	}
	btCompoundShape* compoundShape = new btCompoundShape();
	for (int i = 0; i < node->shapeCount(); i++)
	{
		BShape* shape = node->shape(i);
		btShape* colShape = createCollisionShape(shape);
		btTransform shapeTransform;
		const BVector& origin = shape->position();
		const BQuater& rotate = shape->rotate();
		shapeTransform.setOrigin(btVector3(origin.x(), origin.y(), origin.z()));
		shapeTransform.setRotation(btQuaternion(rotate.x(), rotate.y(), rotate.z(), rotate.w()));
		compoundShape->addChildShape(shapeTransform, colShape);
	}
	btVector3 localInertia(0, 0, 0);
	if (mass != 0)
	{
		compoundShape->calculateLocalInertia(mass, localInertia);
	}
	btRigidBody* body = new btRigidBody();
	body->setMass(mass);
	body->setInertia(localInertia);
	body->setTransform(transform);
	body->setShape(compoundShape);
	body->setName(node->name().cstr());
	body->setAsset(node);
	node->setAsset(body);
	return body;
}

void DynamicScene::slotInsert(BObject* object, const BValue& value)
{
	if (BNode* node = value)
	{
		if (BPhysics* physics = node->physics())
		{
			connect(physics, Signal_Changed, &DynamicScene::slotPhysicsChanged);
			btRigidBody* body = createRigidBody(node, physics);
			_dynamicsWorld->addBody(body);
		}
		return;
	}
	if (BConstraint* constraint = value)
	{
		if (btConstraint* typedConstraint = createTypedConstraint(constraint))
		{
			typedConstraint->setName(constraint->name().cstr());
			typedConstraint->setAsset(constraint);
			typedConstraint->setInnerCollible(constraint->innerCollible());
			BReal breakImpulse = constraint->breakImpulse();
			typedConstraint->setBreakingImpulse(breakImpulse);
			_dynamicsWorld->addConstraint(typedConstraint);
			constraint->setAsset(typedConstraint);
		}
		connect(constraint, Signal_Enabled, &DynamicScene::slotConstraintEnabled);
		connect(constraint, Signal_Changed, &DynamicScene::slotConstraintChanged);
		return;
	}
}
void DynamicScene::slotRemove(BObject* object, const BValue& value)
{
	if (BNode* node = value)
	{
		if (btRigidBody* body = node->asset())
		{
			_dynamicsWorld->removeBody(body);
		}
		return;
	}
	if (BConstraint* constraint = value)
	{
		if (this->contain(constraint))
		{
			btConstraint* typedConstraint = constraint->asset();
			_dynamicsWorld->removeConstraint(typedConstraint);
		}
		return;
	}
}
void DynamicScene::slotConstraintEnabled(BObject* object, const BValue& value)
{
	bool enabled = value;
	if (btConstraint* typedConstraint = object->asset())
	{
		typedConstraint->setEnabled(enabled);
	}
}
void DynamicScene::slotConstraintChanged(BObject* object, const BValue& value)
{
	BConstraint* constraint = (BConstraint*)object;
	btConstraint* typedConstraint = object->asset();
	typedConstraint->setInnerCollible(constraint->innerCollible());
	if (BPointConstraint* pointConstraint = dynamic_cast<BPointConstraint*>(object))
	{
		btPointConstraint* constraint = (btPointConstraint*)typedConstraint;
		constraint->setTAU(pointConstraint->tau());
		constraint->setDamping(pointConstraint->damping());
		constraint->setImpulseClamp(pointConstraint->impulseClamp());
		constraint->setBreakingImpulse(pointConstraint->breakImpulse());
		return;
	}
	if (BHingeConstraint* hingeConstraint = dynamic_cast<BHingeConstraint*>(object))
	{
		btHingeConstraint* constraint = (btHingeConstraint*)typedConstraint;
		const BVector& axisA = hingeConstraint->axisA();
		btVector3 axisInA(axisA.x(), axisA.y(), axisA.z());
		constraint->setAxis(axisInA);
		constraint->enableMotor(hingeConstraint->motorEnabled());
		constraint->setMaxMotorImpulse(hingeConstraint->motorMaxImpulse());
		constraint->setMotorTargetVelocity(hingeConstraint->motorTargetVelocity());
		const BAngleLimit& limit = hingeConstraint->angleLimit();
		BReal low = limit.low();
		BReal high = limit.high();
		BReal softness = limit.softness();
		BReal biasFactor = limit.biasFactor();
		BReal relaxFactor = limit.relaxFactor();
		constraint->setLimit(low, high, softness, biasFactor, relaxFactor);
		return;
	}
	if (BSliderConstraint* sliderConstraint = dynamic_cast<BSliderConstraint*>(object))
	{
		btSliderConstraint* constraint = (btSliderConstraint*)typedConstraint;

		BLine line;
		{
			const BMatrix& matrixA = sliderConstraint->nodeA()->matrix();
			const BRealRange& linearLimit = sliderConstraint->linearLimit();
			line.setBegin((sliderConstraint->pointA() + sliderConstraint->axisA() * linearLimit.min()) * matrixA);
			line.setEnd((sliderConstraint->pointA() + sliderConstraint->axisA() * linearLimit.max()) * matrixA);
		}
	
		const BVector& origin = line.begin();
		btTransform frameInA = btTransform::getIdentity();
		frameInA.setOrigin(btVector3(origin.x(), origin.y(), origin.z()));

		const BVector& pointB = sliderConstraint->pointB();
		btTransform frameInB = btTransform::getIdentity();
		frameInB.setOrigin(btVector3(pointB.x(), pointB.y(), pointB.z()));

		constraint->setFrames(frameInA, frameInB);

		constraint->setLowerLinearLimit(0);
		constraint->setUpperLinearLimit(line.length());

		const BRealRange& angleLimit = sliderConstraint->angleLimit();
		constraint->setLowerAngularLimit(angleLimit.min());
		constraint->setUpperAngularLimit(angleLimit.max());
		return;
	}
	if (BConearConstraint* conearConstraint = dynamic_cast<BConearConstraint*>(object))
	{
		btConeTwistConstraint* constraint = (btConeTwistConstraint*)typedConstraint;
		btRigidBody* bodyA = conearConstraint->nodeA()->asset();
		btRigidBody* bodyB = conearConstraint->nodeB()->asset();

		const BVector& pointA = conearConstraint->pointA();
		const BVector& pointB = conearConstraint->pointB();

		btTransform frameInA = btTransform::getIdentity();
		frameInA.setOrigin(btVector3(pointA.x(), pointA.y(), pointA.z()));
		frameInA.getBasis().setEulerZYX(0, 0, PI / 2);
		btTransform frameInB = btTransform::getIdentity();
		frameInB.setOrigin(btVector3(pointB.x(), pointB.y(), pointB.z()));
		//frameInB.setRotation(btQuaternion(rotateB.x(), rotateB.y(), rotateB.z(), rotateB.w()));
		constraint->setFrames(frameInA, frameInB);

		BReal twistSpan = conearConstraint->twistSpan();
		BReal swingSpan1 = conearConstraint->swingSpan1();
		BReal swingSpan2 = conearConstraint->swingSpan2();
		BReal softness = conearConstraint->softness();
		BReal biasFactor = conearConstraint->biasFactor();
		BReal relaxFactor = conearConstraint->relaxFactor();
		constraint->setLimit(swingSpan1, swingSpan2, twistSpan, softness, biasFactor, relaxFactor);
		return;
	}

}
void DynamicScene::slotPhysicsChanged(BObject* object, const BValue& value)
{
	BPhysics* physics = (BPhysics*)object;
	for (int i = 0; i < this->nodeCount(); i++)
	{
		BNode* node = this->node(i);
		if (node->physics() != physics)
			continue;
		if (btRigidBody* body = node->asset())
		{
			btVector3 localInertia = body->getLocalInertia();
			BReal mass = body->getMass();
			body->setMass(physics->mass());
			body->setInertia(localInertia);
		}
	}
}

void DynamicScene::update(double time)
{
	if (_dynamicsWorld)
	{
		static double tick = 0;
		if (tick > 0)
			_dynamicsWorld->simulate(time - tick);
		else
			tick = time;
		const btBodyArray& objects = _dynamicsWorld->getBodyArray();
		for (int i = 0; i < _dynamicsWorld->getNumBodys(); i++)
		{
			btBody* object = objects[i];
			btRigidBody* body = dynamic_cast<btRigidBody*>(object);
			BNode* node = object->asset();
			if (node && node->enabled())
			{
				btTransform transform = body->getTransform();
				const btVector3& origin = transform.getOrigin();
				btQuaternion rotate = transform.getRotation();
				node->setPosition(origin.x(), origin.y(), origin.z());
				node->setRotate(rotate.x(), rotate.y(), rotate.z(), rotate.w());
			}
			else
			{
				int a = 0;
			}
		}
	}
}
